
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       borad_config.c
  * @author     baiyang
  * @date       2021-10-4
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "borad_config.h"

#include <string.h>

#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <common/console/console.h>
#include <srv_channel/hal/srv_hal_mgr.h>
/*-----------------------------------macro------------------------------------*/
#ifndef BOARD_SAFETY_OPTION_DEFAULT
#define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
#endif

#ifndef BOARD_SAFETY_ENABLE
#define BOARD_SAFETY_ENABLE 1
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static board_config board_cfg;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void brd_init()
{
    board_cfg._in_error_loop = false;
    board_cfg.vehicle_init_stage = INIT_STAGE_HARDWARE;

    brd_board_setup(&board_cfg);

    board_cfg.state.safety_option = BOARD_SAFETY_OPTION_DEFAULT;
    board_cfg.state.ignore_safety_channels = 0;

    board_cfg.vehicle_init_stage = INIT_STAGE_BOARD;
}

void brd_set_vehicle_init_stage(enum init_stage stage)
{
    board_cfg.vehicle_init_stage = stage;
}

enum init_stage  brd_get_vehicle_init_stage()
{
    return board_cfg.vehicle_init_stage;
}

void brd_set_vehicle_init_finish()
{
    board_cfg.vehicle_init_stage = INIT_STAGE_END;
}

bool brd_is_vehicle_init_finish()
{
    return board_cfg.vehicle_init_stage == INIT_STAGE_END;
}

bool brd_is_vehicle_esc_calibrate()
{
    return board_cfg.vehicle_init_stage == INIT_STAGE_ESC_CAL1 || board_cfg.vehicle_init_stage == INIT_STAGE_ESC_CAL2;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
px4_board_type brd_get_board_type()
{
#if GP_FEATURE_BOARD_DETECT
    return board_cfg.px4_configured_board;
#else
    return BOARD_TYPE_UNKNOWN;
#endif
}

void brd_throw_error(const char *err_type, const char *fmt, va_list arg)
{
    board_cfg._in_error_loop = true;
    /*
      to give the user the opportunity to connect to USB we keep
      repeating the error.  The mavlink delay callback is initialised
      before this, so the user can change parameters (and in
      particular BRD_TYPE if needed)
    */
    uint32_t last_print_ms = 0;
    while (true) {
        uint32_t now = time_millis();
        if (now - last_print_ms >= 5000) {
            last_print_ms = now;
            char printfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+2];
            snprintf(printfmt, sizeof(printfmt), "%s: %s\n", err_type, fmt);
            console_printf_textv(printfmt, arg);
            snprintf(printfmt, sizeof(printfmt), "%s: %s", err_type, fmt);
            mavproxy_send_textv(MAV_SEVERITY_CRITICAL, printfmt, arg);
        }

        rt_thread_mdelay(10);
    }
}

void brd_config_error(const char *fmt, ...)
{
    va_list arg_list;
    va_start(arg_list, fmt);
    brd_throw_error("Config Error", fmt, arg_list);
    va_end(arg_list);
}

// ask if IOMCU is enabled. This is a uint8_t to allow
// developer debugging by setting BRD_IO_ENABLE=100 to avoid the
// crc check of IO firmware on startup
uint8_t brd_io_enabled(void)
{
#if HAL_WITH_IO_MCU
    return board_cfg.state.io_enable;
#else
    return 0;
#endif
}

// return safety button options. Bits are in enum board_safety_button_option
uint16_t brd_get_safety_button_options(void)
{
    return (uint16_t)(board_cfg.state.safety_option);
}

// return the value of BRD_SAFETY_MASK
uint16_t brd_get_safety_mask(void)
{
#if GP_FEATURE_BOARD_DETECT || defined(GP_FEATURE_BRD_PWM_COUNT_PARAM)
    return (uint16_t)(board_cfg.state.ignore_safety_channels);
#else
    return 0;
#endif
}

// set default value for BRD_SAFETY_MASK
void brd_set_default_safety_ignore_mask(uint16_t mask)
{
    board_cfg.state.ignore_safety_channels = mask;
}

/*
  handle logic for safety state button press. This should be called at
  10Hz when the button is pressed. The button can either be directly
  on a pin or on a UAVCAN device
  This function returns true if the safety state should be toggled
 */
bool brd_safety_button_handle_pressed(uint8_t press_count)
{
    if (press_count != 10) {
        return false;
    }
    // get button options
    uint16_t safety_options = brd_get_safety_button_options();
    if (!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) &&
        board_cfg.soft_armed) {
        return false;
    }

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    enum safety_state safety_state = srv_hal_safety_switch_state();
#else
    enum safety_state safety_state = SAFETY_NONE;
#endif

    if (safety_state == SAFETY_DISARMED &&
        !(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
        return false;
    }
    if (safety_state == SAFETY_ARMED &&
        !(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
        return false;
    }
    return true;
}

enum safety_state brd_safety_switch_state()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    enum safety_state safety_state = srv_hal_safety_switch_state();
#else
    enum safety_state safety_state = SAFETY_NONE;
#endif

    return safety_state;
}

void brd_set_soft_armed(const bool b)
{
    board_cfg.soft_armed = b;
}

bool brd_get_soft_armed()
{
    return board_cfg.soft_armed;
}

/*------------------------------------test------------------------------------*/


